package estrategia_IO;

/**
 * LAST UPDATE: 13/10
 */

import lejos.hardware.Button;
import lejos.hardware.Sound;
import lejos.hardware.lcd.LCD;
import lejos.hardware.motor.NXTRegulatedMotor;
import lejos.hardware.port.Port;
import lejos.hardware.sensor.EV3ColorSensor;
import lejos.hardware.sensor.HiTechnicColorSensor;
import lejos.robotics.LightScanner;
import lejos.robotics.SampleProvider;
import lejos.utility.Delay;

/**
 * Subsistema de lancamento das bolas.
 * 
 * @author SEK 2014
 * 
 */
public class Lancamento_IO_D implements Runnable {

	// Constantes
	private static final int REPOUSO = 0;
	private static final int ATUALIZANDO = 1;
	private static final int LANCANDO = 2;
	private static final int AJUSTANDO = 3;

	public final boolean DIREITA = false;
	public final boolean ESQUERDA = true;

	public int velBatida;
	private static final int velRetorno = 100;
	private static int angBatida;
	private static int angRetorno;

	private static int angEsquerda = -20;
	private static int angDireita = 20;

	public boolean fim = false;

	// Componentes do robo
	private HiTechnicColorSensor sensorCor;
	private NXTRegulatedMotor motorTaco;

	// Variaveis de estado
	private boolean temBolinha;
	private boolean bolaAzul;
	private boolean bolaLancada;
	private int SubEstado;
	private boolean posicaoTaco;

	/**
	 * Metodo get para a variavel posicaoTaco
	 * 
	 * @param null
	 * @return variavel posicaoTaco
	 */
	public boolean getPosicaoTaco() {
		return this.posicaoTaco;
	}

	/**
	 * Metodo get para a variavel temBolinha
	 * 
	 * @param null
	 * @return variavel temBolinha
	 */
	public boolean getTemBolinha() {
		return this.temBolinha;
	}

	/**
	 * Metodo get para a variavel bolaAzul
	 * 
	 * @param null
	 * @return variavel bolaAzul
	 */
	public boolean getBolaAzul() {
		return this.bolaAzul;
	}

	/**
	 * Metodo get para a variavel bolaLançada
	 * 
	 * @param null
	 * @return variavel bolaLancada
	 */
	public boolean getBolaLancada() {
		return this.bolaLancada;
	}

	public void setVelocidadeBatida(int velBatida) {
		this.velBatida = velBatida;
	}

	public void setAnguloBatida(int angBatida) {
		this.angBatida = angBatida;
	}

	public void setAnguloRetorno(int angRetorno) {
		this.angRetorno = angRetorno;
	}

	/**
	 * Constroi um subsistema de lancamento do robo
	 * 
	 * @param s1
	 *            Porta de ligacao do sensor de cor
	 * @param m1
	 *            Porta de ligacao do motor do taco
	 * @return null
	 */
	public Lancamento_IO_D(Port s1, Port m1) {
		this.sensorCor = new HiTechnicColorSensor(s1);
		this.motorTaco = new NXTRegulatedMotor(m1);

		this.bolaAzul = false;
		this.temBolinha = false;
		this.bolaLancada = true;
		this.posicaoTaco = DIREITA;
		this.SubEstado = REPOUSO;

		motorTaco.rotateTo(angDireita);

		// motorTaco.suspendRegulation();
		Sound.beepSequenceUp();
		// Button.waitForAnyPress();

		LCD.clear();
	}

	/**
	 * Metodo no qual o subsistema de lancamento executara suas rotinas de
	 * amostragem do sensor de cor e atualizacao das variaveis correspondentes
	 */
	@Override
	public void run() {
		// TODO Auto-generated method stub
		while (!fim) {
			amostrarSensorCor();
			Thread.yield();
			Delay.msDelay(1000);
		}
	}

	/**
	 * Metodo de lancamento de bolas laranjas
	 * 
	 * @param null
	 * @return Sucesso ou nao do lancamento
	 */
	public boolean lancarBolaLaranja() {
		while (SubEstado != REPOUSO)
			;
		// motorTaco.resetTachoCount();
		// System.out.println(motorTaco.getTachoCount());
		SubEstado = LANCANDO;
		motorTaco.setSpeed(velBatida);
		if (posicaoTaco == DIREITA) {
			motorTaco.rotate(-angBatida);
			Delay.msDelay(500);
			motorTaco.setSpeed(velRetorno);
			motorTaco.rotate(-angRetorno);
			Delay.msDelay(100);
		} else { // if(posicaoTaco/ == ESQUERDA)
			motorTaco.rotate(angBatida);
			Delay.msDelay(500);
			motorTaco.setSpeed(velRetorno);
			motorTaco.rotate(angRetorno);
			Delay.msDelay(100);
		}
		bolaLancada = true;
		SubEstado = REPOUSO;
		return true;
	}

	/**
	 * Metodo de lancamento de bolas azuis
	 * 
	 * @param null
	 * @return Sucesso ou nao do lancamento
	 */
	public boolean lancarBolaAzul(boolean direcao) {
		while (SubEstado != REPOUSO)
			;
		SubEstado = LANCANDO;
		motorTaco.setSpeed(velRetorno);
		// if (direcao == DIREITA) {
		// motorTaco.rotate(-angRetorno - angBatida);
		// } else { // if(direcao == ESQUERDA)
		motorTaco.rotate(angBatida + angRetorno);
		// }
		bolaLancada = true;
		SubEstado = REPOUSO;
		return true;
	}

	/**
	 * Metodo que efetua a leitura e tratamento do sensor de cor. Ocorre tambem
	 * a atualizacao das variaveis correspondentes.
	 * 
	 * @param null
	 * @return Sucesso ou nao da leitura do sensor
	 */
	private boolean amostrarSensorCor() {
		while (SubEstado != REPOUSO)
			;
		SubEstado = ATUALIZANDO;
		SampleProvider coresRGB = sensorCor.getRGBMode();
		float[] amostras = new float[coresRGB.sampleSize()];
		coresRGB.fetchSample(amostras, 0);

		if (amostras[0] < 0.01 && amostras[2] < 0.01) {
			temBolinha = false;
			Button.LEDPattern(0);
		} else if (amostras[0] >= 0.25) {
			Button.LEDPattern(3);
			bolaAzul = false;
			temBolinha = true;
			bolaLancada = false;
		} else if (amostras[2] >= 0.09) {
			Button.LEDPattern(4);
			bolaAzul = true;
			temBolinha = true;
			bolaLancada = false;
		}
		SubEstado = REPOUSO;
		return true;
	}

	/**
	 * Metodo que ajusta a posicao do taco de acordo com a orientacao do robo
	 * 
	 * @param pos
	 *            posicao na qual o taco deve ser posicionado
	 * @return Sucesso ou nao do reposicionamento
	 */
	public void ajustarTaco(boolean pos) {
		while (SubEstado != REPOUSO)
			;
		SubEstado = AJUSTANDO;
		motorTaco.setSpeed(velRetorno);
		if (pos == DIREITA) {
			motorTaco.rotate(-120 - 2 * angEsquerda);
		} else { // if(pos == ESQUERDA)
			motorTaco.rotate(120 - 2 * angDireita);
		}
		System.out.println(pos);
		System.out.println(motorTaco.getTachoCount());
		// motorTaco.resetTachoCount();
		posicaoTaco = pos;
		SubEstado = REPOUSO;
	}
}